#include "stm32f10x.h"
#include "config.h"
#include "def.h"
#include "types.h"
#include "Serial.h"
#include "Protocol.h"
#include "MultiWii.h"
#include "Alarms.h"
# include "timer.h"
/**************************************************************************************/
/***************             Global RX related variables           ********************/
/**************************************************************************************/
extern int16_t rcData[RC_CHANS];//stm32 add
#if defined(SPEKTRUM)
#include <wiring.c>  //Auto-included by the Arduino core... but we need it sooner. 
#endif

//RAW RC values will be store here
#if defined(SBUS)
volatile uint16_t rcValue[RC_CHANS] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; // interval [1000;2000]
#elif defined(SPEKTRUM) || defined(SERIAL_SUM_PPM)
volatile uint16_t rcValue[RC_CHANS] = {1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502,
									   1502}; // interval [1000;2000]
#else
volatile uint16_t rcValue[RC_CHANS] = {1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502}; // interval [1000;2000]
#endif

#if defined(SERIAL_SUM_PPM) //Channel order for PPM SUM RX Configs
static uint8_t rcChannel[RC_CHANS] = {SERIAL_SUM_PPM};
#elif defined(SBUS) //Channel order for SBUS RX Configs
// for 16 + 2 Channels SBUS. The 10 extra channels 8->17 are not used by MultiWii, but it should be easy to integrate them.
static uint8_t rcChannel[RC_CHANS] = {SBUS};
#elif defined(SUMD)
static uint8_t rcChannel[RC_CHANS] = {PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4};
#elif defined(SPEKTRUM)
static uint8_t rcChannel[RC_CHANS] = {PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4,8,9,10,11};
#else // Standard Channel order
static uint8_t rcChannel[RC_CHANS]  = {ROLLPIN, PITCHPIN, YAWPIN, THROTTLEPIN, AUX1PIN,AUX2PIN,AUX3PIN,AUX4PIN};
static uint8_t PCInt_RX_Pins[PCINT_PIN_COUNT] = {PCINT_RX_BITS}; // if this slowes the PCINT readings we can switch to a define for each pcint bit
#endif

void rxInt(void);

/**************************************************************************************/
/***************                   RX Pin Setup                    ********************/
/**************************************************************************************/
void configureReceiver() {
	/******************    Configure each rc pin for PCINT    ***************************/
	/********超声波加入********************/
	//#if defined(TrigPin)//在定义了管脚之后才应用
	// #if defined(TrigPin)
	//   pinMode(TrigPin, OUTPUT);  //#define TrigPin 4    #define EchoPin 8
	//  pinMode(EchoPin, INPUT);
	//  digitalWrite(EchoPin, LOW);
	//  PCICR  |= (1<<PCIE1);    //引脚(Port C)变化中断控制寄存器,使能中断向量
	// PCMSK1 = (1<<PCINT9);  //A2(PC2)作为回波输入,引脚变化屏蔽寄存器,决定哪些引脚触发中断
	//#endif
// #endif
	/*
	#if defined(STANDARD_RX)
	#if defined(MEGA)
	  DDRK = 0;  // defined PORTK as a digital port ([A8-A15] are consired as digital PINs and not analogical)
	#endif
	// PCINT activation
	for(uint8_t i = 0; i < PCINT_PIN_COUNT; i++){ // i think a for loop is ok for the init.
	  PCINT_RX_PORT |= PCInt_RX_Pins[i];
	  PCINT_RX_MASK |= PCInt_RX_Pins[i];
	}
	PCICR = PCIR_PORT_BIT;
	*/
	/*************    atmega328P's Specific Aux2 Pin Setup    *********************/
#if defined(PROMINI)
#if defined(RCAUXPIN)
	PCICR  |= (1 << 0) ; // PCINT activated also for PINS [D8-D13] on port B
#if defined(RCAUXPIN8)
	PCMSK0 = (1 << 0);
#endif
#if defined(RCAUXPIN12)
	PCMSK0 = (1 << 4);
#endif
#endif
#endif

	/***************   atmega32u4's Specific RX Pin Setup   **********************/
#if defined(PROMICRO)
	//Trottle on pin 7
	DDRE &= ~(1 << 6); // pin 7 to input
	PORTE |= (1 << 6); // enable pullups
	EICRB |= (1 << ISC60);
	EIMSK |= (1 << INT6); // enable interuppt
	// Aux2 pin on PBO (D17/RXLED)
#if defined(RCAUX2PIND17)
	DDRB &= ~(1 << 0); // set D17 to input
#endif
	// Aux2 pin on PD2 (RX0)
#if defined(RCAUX2PINRXO)
	DDRD &= ~(1 << 2); // RX to input
	PORTD |= (1 << 2); // enable pullups
	EICRA |= (1 << ISC20);
	EIMSK |= (1 << INT2); // enable interuppt
#endif
#endif

	/*************************   Special RX Setup   ********************************/
	/*#endif

	#if defined(SERIAL_SUM_PPM)
	  PPM_PIN_INTERRUPT;

	#endif
		*/
#if defined (SPEKTRUM) || defined(SUMD)
	SerialOpen(RX_SERIAL_PORT,115200);
#endif
#if defined(SBUS)
	SerialOpen(RX_SERIAL_PORT,100000);
	switch (RX_SERIAL_PORT) { //parity
#if defined(MEGA)
	case 0:
		UCSR0C |= (1<<UPM01)|(1<<USBS0);
		break;
	case 1:
		UCSR1C |= (1<<UPM11)|(1<<USBS1);
		break;
	case 2:
		UCSR2C |= (1<<UPM21)|(1<<USBS2);
		break;
	case 3:
		UCSR3C |= (1<<UPM31)|(1<<USBS3);
		break;
#endif
	}
#endif
}


//=====中断处理D8管脚的超声波中断====对应第49行的设置=================
/*

ISR( PCINT1_vect){
//处理D8脚上的超声波中断信号中断


   static uint16_t intervalStart;  //只需保留16位即可
   if (digitalRead(EchoPin) == HIGH)  //#define TrigPin 4    #define EchoPin 8
      {
        intervalStart = micros();    // store the current micros() value
        //debug[1]++;
      }
    else
    {
        sonarTime = micros() - intervalStart;
        //distance = (unsigned int)((micros() - intervalStart)*0.017);  //单位是cm  340*100/1000/1000 1us的距离 0.017
        //debug[1]--;
    }
}
*/
/**************************************************************************************/
/***************               Standard RX Pins reading            ********************/
/**************************************************************************************/
#if defined(STANDARD_RX)

#if defined(FAILSAFE) && !defined(PROMICRO)
// predefined PC pin block (thanks to lianj)  - Version with failsafe
#define RX_PIN_CHECK(pin_pos, rc_value_pos)                        \
	if (mask & PCInt_RX_Pins[pin_pos]) {                             \
	  if (!(pin & PCInt_RX_Pins[pin_pos])) {                         \
		dTime = cTime-edgeTime[pin_pos];                             \
		if (900<dTime && dTime<2200) {                               \
		  rcValue[rc_value_pos] = dTime;                             \
		  if((rc_value_pos==THROTTLEPIN || rc_value_pos==YAWPIN ||   \
			  rc_value_pos==PITCHPIN || rc_value_pos==ROLLPIN)       \
			  && dTime>FAILSAFE_DETECT_TRESHOLD)                     \
				GoodPulses |= (1<<rc_value_pos);                     \
		}                                                            \
	  } else edgeTime[pin_pos] = cTime;                              \
	}
#else
// predefined PC pin block (thanks to lianj)  - Version without failsafe
#define RX_PIN_CHECK(pin_pos, rc_value_pos)                        \
	if (mask & PCInt_RX_Pins[pin_pos]) {                             \
	  if (!(pin & PCInt_RX_Pins[pin_pos])) {                         \
		dTime = cTime-edgeTime[pin_pos];                             \
		if (900<dTime && dTime<2200) {                               \
		  rcValue[rc_value_pos] = dTime;                             \
		}                                                            \
	  } else edgeTime[pin_pos] = cTime;                              \
	}
#endif

// port change Interrupt
ISR(RX_PC_INTERRUPT) { //this ISR is common to every receiver channel, it is call everytime a change state occurs on a RX input pin
	uint8_t mask;
	uint8_t pin;
	uint16_t cTime,dTime;
	static uint16_t edgeTime[8];
	static uint8_t PCintLast;
#if defined(FAILSAFE) && !defined(PROMICRO)
	static uint8_t GoodPulses;
#endif

	pin = RX_PCINT_PIN_PORT; // RX_PCINT_PIN_PORT indicates the state of each PIN for the arduino port dealing with Ports digital pins

	mask = pin ^ PCintLast;   // doing a ^ between the current interruption and the last one indicates wich pin changed
	cTime = micros();         // micros() return a uint32_t, but it is not usefull to keep the whole bits => we keep only 16 bits
	sei();                    // re enable other interrupts at this point, the rest of this interrupt is not so time critical and can be interrupted safely
	PCintLast = pin;          // we memorize the current state of all PINs [D0-D7]

#if (PCINT_PIN_COUNT > 0)
	RX_PIN_CHECK(0,2);
#endif
#if (PCINT_PIN_COUNT > 1)
	RX_PIN_CHECK(1,4);
#endif
#if (PCINT_PIN_COUNT > 2)
	RX_PIN_CHECK(2,5);
#endif
#if (PCINT_PIN_COUNT > 3)
	RX_PIN_CHECK(3,6);
#endif
#if (PCINT_PIN_COUNT > 4)
	RX_PIN_CHECK(4,7);
#endif
#if (PCINT_PIN_COUNT > 5)
	RX_PIN_CHECK(5,0);
#endif
#if (PCINT_PIN_COUNT > 6)
	RX_PIN_CHECK(6,1);
#endif
#if (PCINT_PIN_COUNT > 7)
	RX_PIN_CHECK(7,3);
#endif

#if defined(FAILSAFE) && !defined(PROMICRO)
	if (GoodPulses==(1<<THROTTLEPIN)+(1<<YAWPIN)+(1<<ROLLPIN)+(1<<PITCHPIN)) {  // If all main four chanells have good pulses, clear FailSafe counter
		GoodPulses = 0;
		if(failsafeCnt > 20) failsafeCnt -= 20;
		else failsafeCnt = 0;
	}
#endif
}
/*********************      atmega328P's Aux2 Pins      *************************/
#if defined(PROMINI)
#if defined(RCAUXPIN)
/* this ISR is a simplification of the previous one for PROMINI on port D
   it's simplier because we know the interruption deals only with one PIN:
   bit 0 of PORT B, ie Arduino PIN 8
   or bit 4 of PORTB, ie Arduino PIN 12
 => no need to check which PIN has changed */
ISR(PCINT0_vect) {
	uint8_t pin;
	uint16_t cTime,dTime;
	static uint16_t edgeTime;

	pin = PINB;
	cTime = micros();
	sei();
#if defined(RCAUXPIN8)
	if (!(pin & 1<<0)) {     //indicates if the bit 0 of the arduino port [B0-B7] is not at a high state (so that we match here only descending PPM pulse)
#endif
#if defined(RCAUXPIN12)
		if (!(pin & 1<<4)) {     //indicates if the bit 4 of the arduino port [B0-B7] is not at a high state (so that we match here only descending PPM pulse)
#endif
			dTime = cTime-edgeTime;
			if (900<dTime && dTime<2200) rcValue[0] = dTime; // just a verification: the value must be in the range [1000;2000] + some margin
		} else edgeTime = cTime;    // if the bit 2 is at a high state (ascending PPM pulse), we memorize the time
	}
#endif
#endif

	/****************      atmega32u4's Throttle & Aux2 Pin      *******************/
#if defined(PROMICRO)
	// throttle
	ISR(INT6_vect) {
		static uint16_t now,diff;
		static uint16_t last = 0;
		now = micros();
		if(!(PINE & (1<<6))) {
			diff = now - last;
			if(900<diff && diff<2200) {
				rcValue[3] = diff;
#if defined(FAILSAFE)
				if(diff>FAILSAFE_DETECT_TRESHOLD) {        // if Throttle value is higher than FAILSAFE_DETECT_TRESHOLD
					if(failsafeCnt > 20) failsafeCnt -= 20;
					else failsafeCnt = 0;   // If pulse present on THROTTLE pin (independent from ardu version), clear FailSafe counter  - added by MIS
				}
#endif
			}
		} else last = now;
	}
	// Aux 2
#if defined(RCAUX2PINRXO)
	ISR(INT2_vect) {
		static uint16_t now,diff;
		static uint16_t last = 0;
		now = micros();
		if(!(PIND & (1<<2))) {
			diff = now - last;
			if(900<diff && diff<2200) rcValue[7] = diff;
		} else last = now;
	}
#endif
#endif
#endif


/**************************************************************************************/
/***************                PPM SUM RX Pin reading             ********************/
/**************************************************************************************/
//printf("左右:%5d 前后:%5d 油门:%5d 航向:%5d chanel5:%5d chanel6:%5d  chanel7:%5d chanel8:%5d chanel9:%5d chanel0:%5d\r\n",ppm_rx[1]+403,ppm_rx[2]+403,ppm_rx[3]+403,ppm_rx[4]+403,ppm_rx[5]+403,ppm_rx[6]+403,ppm_rx[7]+403,ppm_rx[8]+403,ppm_rx[9]+403,ppm_rx[11]+403);
/*
// attachInterrupt fix for promicro
#if defined(PROMICRO) && defined(SERIAL_SUM_PPM)
ISR(INT6_vect){rxInt();}
#endif

// PPM_SUM at THROTTLE PIN on MEGA boards
#if defined(PPM_ON_THROTTLE) && defined(MEGA) && defined(SERIAL_SUM_PPM)
ISR(PCINT2_vect) { if(PINK & (1<<0)) rxInt(); }
#endif

// Read PPM SUM RX Data
#if defined(SERIAL_SUM_PPM)
void rxInt(void) {
uint16_t now,diff;
static uint16_t last = 0;
static uint8_t chan = 0;
#if defined(FAILSAFE)
static uint8_t GoodPulses;
#endif

now = micros();
sei();
diff = now - last;
last = now;
if(diff>3000) chan = 0;
else {
if(900<diff && diff<2200 && chan<RC_CHANS ) {   //Only if the signal is between these values it is valid, otherwise the failsafe counter should move up
rcValue[chan] = diff;
#if defined(FAILSAFE)
if(chan<4 && diff>FAILSAFE_DETECT_TRESHOLD) GoodPulses |= (1<<chan); // if signal is valid - mark channel as OK
if(GoodPulses==0x0F) {                                               // If first four chanells have good pulses, clear FailSafe counter
GoodPulses = 0;
if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0;
}
#endif
}
chan++;
}
}
#endif
*/

/**************************************************************************************/
/***************                   SBUS RX Data                    ********************/
/**************************************************************************************/
#if defined(SBUS)

#define SBUS_SYNCBYTE 0x0F // Not 100% sure: at the beginning of coding it was 0xF0 !!!
static uint16_t sbusIndex=0;
static uint16_t sbus[25]= {0};

void readSerial_RX() {
	while(SerialAvailable(RX_SERIAL_PORT)) {
		int val = SerialRead(RX_SERIAL_PORT);
		if(sbusIndex==0 && val != SBUS_SYNCBYTE)
			continue;
		sbus[sbusIndex++] = val;
		if(sbusIndex==25) {
			sbusIndex=0;
			spekFrameFlags = 0x00;
			rcValue[0]  = ((sbus[1]|sbus[2]<< 8) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[1]  = ((sbus[2]>>3|sbus[3]<<5) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[2]  = ((sbus[3]>>6|sbus[4]<<2|sbus[5]<<10) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[3]  = ((sbus[5]>>1|sbus[6]<<7) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[4]  = ((sbus[6]>>4|sbus[7]<<4) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[5]  = ((sbus[7]>>7|sbus[8]<<1|sbus[9]<<9) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[6]  = ((sbus[9]>>2|sbus[10]<<6) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[7]  = ((sbus[10]>>5|sbus[11]<<3) & 0x07FF)/2+SBUS_MID_OFFSET; // & the other 8 + 2 channels if you need them
			//The following lines: If you need more than 8 channels, max 16 analog + 2 digital. Must comment the not needed channels!
			rcValue[8]  = ((sbus[12]|sbus[13]<< 8) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[9]  = ((sbus[13]>>3|sbus[14]<<5) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[10] = ((sbus[14]>>6|sbus[15]<<2|sbus[16]<<10) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[11] = ((sbus[16]>>1|sbus[17]<<7) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[12] = ((sbus[17]>>4|sbus[18]<<4) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[13] = ((sbus[18]>>7|sbus[19]<<1|sbus[20]<<9) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[14] = ((sbus[20]>>2|sbus[21]<<6) & 0x07FF)/2+SBUS_MID_OFFSET;
			rcValue[15] = ((sbus[21]>>5|sbus[22]<<3) & 0x07FF)/2+SBUS_MID_OFFSET;
			// now the two Digital-Channels
			if ((sbus[23]) & 0x0001)       rcValue[16] = 2000;
			else rcValue[16] = 1000;
			if ((sbus[23] >> 1) & 0x0001)  rcValue[17] = 2000;
			else rcValue[17] = 1000;
			spekFrameDone = 0x01;

			// Failsafe: there is one Bit in the SBUS-protocol (Byte 25, Bit 4) whitch is the failsafe-indicator-bit
#if defined(FAILSAFE)
			if (!((sbus[23] >> 3) & 0x0001))
			{
				if(failsafeCnt > 20) failsafeCnt -= 20;    // clear FailSafe counter
				else failsafeCnt = 0;
			}
#endif

			// For some reason the SBUS data provides only about 75% of the actual RX output pulse width
			// Adjust the actual value by +/-25%.  Sign determined by pulse width above or below center
			uint8_t adj_index;
			for(adj_index=0; adj_index<16; adj_index++) {
				if (rcValue[adj_index] < MIDRC)
					rcValue[adj_index] -= (MIDRC - rcValue[adj_index]) >> 2;
				else
					rcValue[adj_index] += (rcValue[adj_index] - MIDRC) >> 2;
			}
		}
	}
}
#endif

/**************************************************************************************/
/*************** SUMD ********************/
/**************************************************************************************/

#if defined(SUMD)
#define SUMD_SYNCBYTE 0xA8
#define SUMD_MAXCHAN 8
#define SUMD_BUFFSIZE SUMD_MAXCHAN*2 + 5 // 6 channels + 5 -> 17 bytes for 6 channels
static uint8_t sumdIndex=0;
static uint8_t sumdSize=0;
static uint8_t sumd[SUMD_BUFFSIZE]= {0};

void readSerial_RX(void) {
	while (SerialAvailable(RX_SERIAL_PORT)) {
		int val = SerialRead(RX_SERIAL_PORT);
		if(sumdIndex == 0 && val != SUMD_SYNCBYTE) continue;
		if(sumdIndex == 2) sumdSize = val;
		if(sumdIndex < SUMD_BUFFSIZE) sumd[sumdIndex] = val;
		sumdIndex++;

		if(sumdIndex == sumdSize*2+5) {
			sumdIndex = 0;
			spekFrameFlags = 0x00;
			//debug[1] = sumd[1];
			if (sumdSize > SUMD_MAXCHAN) sumdSize = SUMD_MAXCHAN;
			for (uint8_t b = 0; b < sumdSize; b++)
				rcValue[b] = ((sumd[2*b+3]<<8) | sumd[2*b+4])>>3;
			spekFrameDone = 0x01; // havent checked crc at all

#if defined(FAILSAFE)
			if (sumd[1] == 0x01)
			{
				if(failsafeCnt > 20) failsafeCnt -= 20;    // clear FailSafe counter
				else failsafeCnt = 0;
			}
#endif
		}
	}
}
#endif

/**************************************************************************************/
/***************          combine and sort the RX Datas            ********************/
/**************************************************************************************/
#if defined(SPEKTRUM)
void readSerial_RX(void) {//处理串口接收
	if ((!f.ARMED) &&
#if defined(FAILSAFE) || (RX_SERIAL_PORT != 0)
			(failsafeCnt > 5) &&
#endif
			( SerialPeek(RX_SERIAL_PORT) == '$')) {
		while (SerialAvailable(RX_SERIAL_PORT)) {
			serialCom();
			delay (10);
		}
		return;
	} //End of: Is it the GUI?
	while (SerialAvailable(RX_SERIAL_PORT) > SPEK_FRAME_SIZE) { // More than a frame?  More bytes implies we weren't called for multiple frame times.  We do not want to process 'old' frames in the buffer.
		for (uint8_t i = 0; i < SPEK_FRAME_SIZE; i++) {
			SerialRead(RX_SERIAL_PORT);   //Toss one full frame of bytes.
		}
	}
	if (spekFrameFlags == 0x01) {   //The interrupt handler saw at least one valid frame start since we were last here.
		if (SerialAvailable(RX_SERIAL_PORT) == SPEK_FRAME_SIZE) {  //A complete frame? If not, we'll catch it next time we are called.
			SerialRead(RX_SERIAL_PORT);
			SerialRead(RX_SERIAL_PORT);        //Eat the header bytes
			for (uint8_t b = 2; b < SPEK_FRAME_SIZE; b += 2) {
				uint8_t bh = SerialRead(RX_SERIAL_PORT);
				uint8_t bl = SerialRead(RX_SERIAL_PORT);
				uint8_t spekChannel = 0x0F & (bh >> SPEK_CHAN_SHIFT);
				if (spekChannel < RC_CHANS) rcValue[spekChannel] = 988 + ((((uint16_t)(bh & SPEK_CHAN_MASK) << 8) + bl) SPEK_DATA_SHIFT);
			}
			spekFrameFlags = 0x00;
			spekFrameDone = 0x01;
#if defined(FAILSAFE)
			if(failsafeCnt > 20) failsafeCnt -= 20;
			else failsafeCnt = 0;   // Valid frame, clear FailSafe counter
#endif
		} else { //Start flag is on, but not enough bytes means there is an incomplete frame in buffer.  This could be OK, if we happened to be called in the middle of a frame.  Or not, if it has been a while since the start flag was set.
			uint32_t spekInterval = (timer0_overflow_count << 8) * (64 / clockCyclesPerMicrosecond()) - spekTimeLast;
			if (spekInterval > 2500) {
				spekFrameFlags = 0;   //If it has been a while, make the interrupt handler start over.
			}
		}
	}
}
#endif
extern u16 ppm_rx[12];


#ifdef PPM_AT9
#define PPM_ADD_TIME 505
#endif

#ifdef PPM_FSI6
#define PPM_ADD_TIME 403
#endif

uint16_t readRawRC(uint8_t chan) {
	uint16_t data;
	extern u16 ppm_rx[12];
#if defined(SPEKTRUM) || defined(SBUS) || defined(SUMD)
	if (chan < RC_CHANS) {
		data = rcValue[rcChannel[chan]];
	} else data = 1500;
#else
	//uint8_t oldSREG;
	// oldSREG = SREG;
	///cli(); // Let's disable interrupts

	//data = rcValue[rcChannel[chan]]; // Let's copy the data Atomically
	data = ppm_rx[rcChannel[chan] + 1] + PPM_ADD_TIME;//1不是遥控器通道，是标志变量
	// SREG = oldSREG;        // Let's restore interrupt state
#endif
	return data; // We return the value correctly copied when the IRQ's where disabled
}

/**************************************************************************************/
/***************          compute and Filter the RX data           ********************/
/**************************************************************************************/
#define AVERAGING_ARRAY_LENGTH 4

void computeRC() {
	static uint16_t rcData4Values[RC_CHANS][AVERAGING_ARRAY_LENGTH - 1];
	uint16_t rcDataMean, rcDataTmp;
	static uint8_t rc4ValuesIndex = 0;
	uint8_t chan, a;
	uint8_t failsafeGoodCondition = 1;

#if !defined(OPENLRSv2MULTI)
	rc4ValuesIndex++;
	if (rc4ValuesIndex == AVERAGING_ARRAY_LENGTH - 1) rc4ValuesIndex = 0;
	for (chan = 0; chan < RC_CHANS; chan++) {
		rcDataTmp = readRawRC(chan);
#if defined(FAILSAFE)
		failsafeGoodCondition = rcDataTmp>FAILSAFE_DETECT_TRESHOLD || chan > 3 || !f.ARMED; // update controls channel only if pulse is above FAILSAFE_DETECT_TRESHOLD
#endif                                                                                // In disarmed state allow always update for easer configuration.
#if defined(SPEKTRUM) || defined(SBUS) || defined(SUMD) // no averaging for Spektrum & SBUS & SUMD signal
		if(failsafeGoodCondition)  rcData[chan] = rcDataTmp;
#else
		if (failsafeGoodCondition) {
			rcDataMean = rcDataTmp;
			for (a = 0; a < AVERAGING_ARRAY_LENGTH - 1; a++) rcDataMean += rcData4Values[chan][a];
			rcDataMean = (rcDataMean + (AVERAGING_ARRAY_LENGTH / 2)) / AVERAGING_ARRAY_LENGTH;
			if (rcDataMean < (uint16_t) rcData[chan] - 3) rcData[chan] = rcDataMean + 2;
			if (rcDataMean > (uint16_t) rcData[chan] + 3) rcData[chan] = rcDataMean - 2;
			rcData4Values[chan][rc4ValuesIndex] = rcDataTmp;
		}
#endif
		if (chan < 8 &&
			rcSerialCount > 0) { // rcData comes from MSP and overrides RX Data until rcSerialCount reaches 0
			rcSerialCount--;
#if defined(FAILSAFE)
			failsafeCnt = 0;
#endif
			if (rcSerial[chan] > 900) {
				rcData[chan] = rcSerial[chan];   // only relevant channels are overridden
			}
		} else {
			1 == 1;
		}

	}
#endif
}


/**************************************************************************************/
/***************                     OPENLRS                       ********************/
/**************************************************************************************/

//note: this dont feels right in RX.pde

#if defined(OPENLRSv2MULTI)
// **********************************************************
// ******************   OpenLRS Rx Code   *******************
// ***  OpenLRS Designed by Melih Karakelle on 2010-2012  ***
// **  an Arudino based RC Rx/Tx system with extra futures **
// **       This Source code licensed under GPL            **
// **********************************************************
// Version Number     : 1.11
// Latest Code Update : 2012-03-25
// Supported Hardware : OpenLRS Rx boards (store.flytron.com)
// Project Forum      : http://forum.flytron.com/viewforum.php?f=7
// Google Code Page   : http://code.google.com/p/openlrs/
// **********************************************************
// # PROJECT DEVELOPERS #
// Melih Karakelle (http://www.flytron.com) (forum nick name: Flytron)
// Jan-Dirk Schuitemaker (http://www.schuitemaker.org/) (forum nick name: CrashingDutchman)
// Etienne Saint-Paul (http://www.gameseed.fr) (forum nick name: Etienne)
//

//######### TRANSMISSION VARIABLES ##########
#define CARRIER_FREQUENCY 435000  // 435Mhz startup frequency
#define FREQUENCY_HOPPING 1 // 1 = Enabled  0 = Disabled

//###### HOPPING CHANNELS #######
//Select the hopping channels between 0-255
// Default values are 13,54 and 23 for all transmitters and receivers, you should change it before your first flight for safety.
//Frequency = CARRIER_FREQUENCY + (StepSize(60khz)* Channel_Number)
	static uint8_t hop_list[3] = {13,54,23};

//###### RF DEVICE ID HEADERS #######
// Change this 4 byte values for isolating your transmission, RF module accepts only data with same header
	static uint8_t RF_Header[4] = {'O','L','R','S'};

//########## Variables #################

	static uint32_t last_hopping_time;
	static uint8_t RF_Rx_Buffer[17];
	static uint16_t temp_int;
	static uint16_t Servo_Buffer[10] = {3000,3000,3000,3000,3000,3000,3000,3000};   //servo position values from RF
	static uint8_t hopping_channel = 1;


// **********************************************************
// **      RFM22B/Si4432 control functions for OpenLRS     **
// **       This Source code licensed under GPL            **
// **********************************************************
// Latest Code Update : 2011-09-26
// Supported Hardware : OpenLRS Tx/Rx boards (store.flytron.com)
// Project Forum      : http://forum.flytron.com/viewforum.php?f=7
// Google Code Page   : http://code.google.com/p/openlrs/
// **********************************************************

//*****************************************************************************
//*****************************************************************************
	unsigned char ItStatus1, ItStatus2;

//--------------------------------------------------------------
	void Write0( void ) {
		SCK_off;
		NOP();
		SDI_off;
		NOP();
		SCK_on;
		NOP();
	}
//--------------------------------------------------------------
	void Write1( void ) {
		SCK_off;
		NOP();
		SDI_on;
		NOP();
		SCK_on;
		NOP();
	}
//--------------------------------------------------------------
	void Write8bitcommand(uint8_t command) {  // keep sel to low
		uint8_t n=8;
		nSEL_on;
		SCK_off;
		nSEL_off;
		while(n--) {
			if(command&0x80)
				Write1();
			else
				Write0();
			command = command << 1;
		}
		SCK_off;
	}

//--------------------------------------------------------------
	void send_read_address(uint8_t i) {
		i &= 0x7f;
		Write8bitcommand(i);
	}

//--------------------------------------------------------------
	void send_8bit_data(uint8_t i) {
		uint8_t n = 8;
		SCK_off;
		while(n--) {
			if(i&0x80)
				Write1();
			else
				Write0();
			i = i << 1;
		}
		SCK_off;
	}
//--------------------------------------------------------------

	uint8_t read_8bit_data(void) {
		uint8_t Result, i;

		SCK_off;
		Result=0;
		for(i=0; i<8; i++) {                  //read fifo data byte
			Result=Result<<1;
			SCK_on;
			NOP();
			if(SDO_1) {
				Result|=1;
			}
			SCK_off;
			NOP();
		}
		return(Result);
	}

//--------------------------------------------------------------
	uint8_t _spi_read(uint8_t address) {
		uint8_t result;
		send_read_address(address);
		result = read_8bit_data();
		nSEL_on;
		return(result);
	}

//--------------------------------------------------------------
	void _spi_write(uint8_t address, uint8_t data) {
		address |= 0x80;
		Write8bitcommand(address);
		send_8bit_data(data);
		nSEL_on;
	}

//-------Defaults 38.400 baud----------------------------------------------
	void RF22B_init_parameter(void) {
		ItStatus1 = _spi_read(0x03); // read status, clear interrupt
		ItStatus2 = _spi_read(0x04);
		_spi_write(0x06, 0x00);    // no wakeup up, lbd,
		_spi_write(0x07, RF22B_PWRSTATE_READY);      // disable lbd, wakeup timer, use internal 32768,xton = 1; in ready mode
		_spi_write(0x09, 0x7f);  // c = 12.5p
		_spi_write(0x0a, 0x05);
		_spi_write(0x0b, 0x12);    // gpio0 TX State
		_spi_write(0x0c, 0x15);    // gpio1 RX State
		_spi_write(0x0d, 0xfd);    // gpio 2 micro-controller clk output
		_spi_write(0x0e, 0x00);    // gpio    0, 1,2 NO OTHER FUNCTION.
		_spi_write(0x70, 0x00);    // disable manchest

		// 57.6Kbps data rate
		_spi_write(0x1c, 0x05); // case RATE_57.6K
		_spi_write(0x20, 0x45);//  0x20 calculate from the datasheet= 500*(1+2*down3_bypass)/(2^ndec*RB*(1+enmanch))
		_spi_write(0x21, 0x01); // 0x21 , rxosr[10--8] = 0; stalltr = (default), ccoff[19:16] = 0;
		_spi_write(0x22, 0xD7); // 0x22    ncoff =5033 = 0x13a9
		_spi_write(0x23, 0xDC); // 0x23
		_spi_write(0x24, 0x03); // 0x24
		_spi_write(0x25, 0xB8); // 0x25
		_spi_write(0x2a, 0x1e);

		_spi_write(0x6e, 0x0E); //case RATE_57.6K
		_spi_write(0x6f, 0xBF); //case RATE_57.6K

		_spi_write(0x30, 0x8c);    // enable packet handler, msb first, enable crc,

		_spi_write(0x32, 0xf3);    // 0x32address enable for headere byte 0, 1,2,3, receive header check for byte 0, 1,2,3
		_spi_write(0x33, 0x42);    // header 3, 2, 1,0 used for head length, fixed packet length, synchronize word length 3, 2,
		_spi_write(0x34, 0x07);    // 7 default value or   // 64 nibble = 32byte preamble
		_spi_write(0x36, 0x2d);    // synchronize word
		_spi_write(0x37, 0xd4);
		_spi_write(0x38, 0x00);
		_spi_write(0x39, 0x00);
		_spi_write(0x3a, RF_Header[0]);    // tx header
		_spi_write(0x3b, RF_Header[1]);
		_spi_write(0x3c, RF_Header[2]);
		_spi_write(0x3d, RF_Header[3]);
		_spi_write(0x3e, 17);    // total tx 17 byte

		//RX HEADER
		_spi_write(0x3f, RF_Header[0]);   // check hearder
		_spi_write(0x40, RF_Header[1]);
		_spi_write(0x41, RF_Header[2]);
		_spi_write(0x42, RF_Header[3]);
		_spi_write(0x43, 0xff);    // all the bit to be checked
		_spi_write(0x44, 0xff);    // all the bit to be checked
		_spi_write(0x45, 0xff);    // all the bit to be checked
		_spi_write(0x46, 0xff);    // all the bit to be checked

		_spi_write(0x6d, 0x07); // 7 set power max power
		_spi_write(0x79, 0x00);    // no hopping
		_spi_write(0x7a, 0x06);    // 60khz step size (10khz x value) // no hopping

		_spi_write(0x71, 0x23); // Gfsk, fd[8] =0, no invert for Tx/Rx data, fifo mode, txclk -->gpio
		//_spi_write(0x72, 0x1F); // frequency deviation setting to 19.6khz (for 38.4kbps)
		_spi_write(0x72, 0x2E); // frequency deviation setting to 28.8khz(for 57.6kbps)
		_spi_write(0x73, 0x00);
		_spi_write(0x74, 0x00);    // no offset

		//band 435.000
		_spi_write(0x75, 0x53);
		_spi_write(0x76, 0x7D);
		_spi_write(0x77, 0x00);
	}


	void checkPots() {
		////Flytron OpenLRS Multi Pots
		pot_P = analogRead(7);
		pot_I = analogRead(6);

		pot_P = pot_P - 512;
		pot_I = pot_I - 512;

		pot_P = pot_P / 25; //+-20
		pot_I = pot_I / 25; //+-20
	}

	void initOpenLRS(void) {
		pinMode(GREEN_LED_pin, OUTPUT);
		pinMode(RED_LED_pin, OUTPUT);

		//RF module pins
		pinMode(SDO_pin, INPUT); //SDO
		pinMode(SDI_pin, OUTPUT); //SDI
		pinMode(SCLK_pin, OUTPUT); //SCLK
		pinMode(IRQ_pin, INPUT); //IRQ
		pinMode(nSel_pin, OUTPUT); //nSEL
		checkPots(); // OpenLRS Multi board hardware pot check;
	}

//-----------------------------------------------------------------------
	void rx_reset(void) {
		_spi_write(0x07, RF22B_PWRSTATE_READY);
		_spi_write(0x7e, 36);    // threshold for rx almost full, interrupt when 1 byte received
		_spi_write(0x08, 0x03);    //clear fifo disable multi packet
		_spi_write(0x08, 0x00);    // clear fifo, disable multi packet
		_spi_write(0x07,RF22B_PWRSTATE_RX );  // to rx mode
		_spi_write(0x05, RF22B_Rx_packet_received_interrupt);
		ItStatus1 = _spi_read(0x03);  //read the Interrupt Status1 register
		ItStatus2 = _spi_read(0x04);
	}
//-----------------------------------------------------------------------

//--------------------------------------------------------------
	void to_ready_mode(void) {
		ItStatus1 = _spi_read(0x03);
		ItStatus2 = _spi_read(0x04);
		_spi_write(0x07, RF22B_PWRSTATE_READY);
	}

	void to_rx_mode(void) {
		to_ready_mode();
		delay(50);
		rx_reset();
		NOP();
	}

//--------------------------------------------------------------
	void to_sleep_mode(void) {
		//  TXEN = RXEN = 0;
		//LED_RED = 0;
		_spi_write(0x07, RF22B_PWRSTATE_READY);

		ItStatus1 = _spi_read(0x03);  //read the Interrupt Status1 register
		ItStatus2 = _spi_read(0x04);
		_spi_write(0x07, RF22B_PWRSTATE_POWERDOWN);
	}
//--------------------------------------------------------------

	void frequency_configurator(uint32_t frequency) {
		// frequency formulation from Si4432 chip's datasheet
		// original formulation is working with mHz values and floating numbers, I replaced them with kHz values.
		frequency = frequency / 10;
		frequency = frequency - 24000;
		frequency = frequency - 19000; // 19 for 430-439.9 MHz band from datasheet
		frequency = frequency * 64; // this is the Nominal Carrier Frequency (fc) value for register setting

		uint8_t byte0 = (uint8_t) frequency;
		uint8_t byte1 = (uint8_t) (frequency >> 8);

		_spi_write(0x76, byte1);
		_spi_write(0x77, byte0);
	}

//############# FREQUENCY HOPPING FUNCTIONS #################
#if (FREQUENCY_HOPPING==1)
	void Hopping(void) {
		hopping_channel++;
		if (hopping_channel>2) hopping_channel = 0;
		_spi_write(0x79, hop_list[hopping_channel]);
	}
#endif

	void Config_OpenLRS() {
		RF22B_init_parameter(); // Configure the RFM22B's registers
		frequency_configurator(CARRIER_FREQUENCY); // Calibrate the RFM22B to this frequency, frequency hopping starts from here.
		to_rx_mode();
#if (FREQUENCY_HOPPING==1)
		Hopping(); //Hop to the next frequency
#endif
	}

//############ MAIN LOOP ##############
	void Read_OpenLRS_RC() {
		uint8_t i,tx_data_length;
		uint8_t first_data = 0;

		if (_spi_read(0x0C)==0) {
			RF22B_init_parameter();    // detect the locked module and reboot
			to_rx_mode();
		}
		if ((currentTime-last_hopping_time > 25000)) {//automatic hopping for clear channel when rf link down for 25ms.
			Red_LED_ON;
			last_hopping_time = currentTime;
#if (FREQUENCY_HOPPING==1)
			Hopping(); //Hop to the next frequency
#endif
		}
		if(nIRQ_0) { // RFM22B INT pin Enabled by received Data
			Red_LED_ON;
			send_read_address(0x7f); // Send the package read command
			for(i = 0; i<17; i++) {//read all buffer
				RF_Rx_Buffer[i] = read_8bit_data();
			}
			rx_reset();
			if (RF_Rx_Buffer[0] == 'S') {// servo control data
				for(i = 0; i<8; i++) {//Write into the Servo Buffer
					temp_int = (256*RF_Rx_Buffer[1+(2*i)]) + RF_Rx_Buffer[2+(2*i)];
					if ((temp_int>1500) && (temp_int<4500)) Servo_Buffer[i] = temp_int/2;
				}
				rcData[ROLL] = Servo_Buffer[0];
				rcData[PITCH] = Servo_Buffer[1];
				rcData[THROTTLE] = Servo_Buffer[2];
				rcData[YAW] = Servo_Buffer[3];
				rcData[AUX1] = Servo_Buffer[4];
				rcData[AUX2] = Servo_Buffer[5];
				rcData[AUX3] = Servo_Buffer[6];
				rcData[AUX4] = Servo_Buffer[7];
			}
#if (FREQUENCY_HOPPING==1)
			Hopping(); //Hop to the next frequency
#endif
			delay(1);
			last_hopping_time = currentTime;
			Red_LED_OFF;
		}
		Red_LED_OFF;
	}
#endif


#if defined(SPEK_BIND)  // Bind Support
void spekBind() {
	pinMode(SPEK_BIND_DATA, INPUT);     // Data line from sat
	digitalWrite(SPEK_BIND_DATA,LOW);   // Turn off internal Pull Up resistor

	pinMode(SPEK_BIND_GROUND, INPUT);
	digitalWrite(SPEK_BIND_GROUND,LOW);
	pinMode(SPEK_BIND_GROUND, OUTPUT);
	digitalWrite(SPEK_BIND_GROUND,LOW);

	pinMode(SPEK_BIND_POWER, INPUT);
	digitalWrite(SPEK_BIND_POWER,LOW);
	pinMode(SPEK_BIND_POWER,OUTPUT);

	while(1) {  //Do not return.  User presses reset button to return to normal.
		blinkLED(4,255,1);
		digitalWrite(SPEK_BIND_POWER,LOW); // Power off sat
		pinMode(SPEK_BIND_DATA, OUTPUT);
		digitalWrite(SPEK_BIND_DATA,LOW);
		delay(1000);
		blinkLED(4,255,1);

		digitalWrite(SPEK_BIND_POWER,HIGH); // Power on sat
		delay(10);
		digitalWrite(SPEK_BIND_DATA,HIGH);
		delay(60);                 // Keep data pin steady for 20 to 120ms after power up

		noInterrupts();
		for (byte i = 0; i < SPEK_BIND_PULSES; i++) {
			digitalWrite(SPEK_BIND_DATA,LOW);
			delayMicroseconds(118);
			digitalWrite(SPEK_BIND_DATA,HIGH);
			delayMicroseconds(122);
		}
		interrupts();
		delay(60000);         //Allow one full minute to bind, then try again.
	}
}
#endif
